﻿using Microsoft.Kinect;
using System;
using System.Collections.Generic;
using System.ComponentModel;
using System.Data;
using System.Drawing;
using System.Linq;
using System.Text;
using System.Windows.Forms;


using System.Runtime.InteropServices;
using System.Drawing.Imaging;
using System.Threading;


namespace AlcoKinect
{
    public partial class AppWindow : Form
    {
        Test test;
        KinectSensor sensor;
        Bitmap bmap;
        SoundPlayer alarmPlayer;
        Thread alarmThread;
        Dictionary<String, String> parameters;
        String backgroundImagePath;
        long previousTiltAdjTime;
        bool showResultBackground;

        int legExpectedAngle;
        int legDevAngleThreshold;
        int hipCenterPositionY;
        int hipCenterDevThreshold;
        int arrowPositionY;
        int arrowPoitionYLimit;
        int tiltMinAngle;
        bool adjustSensorTilt;
        bool showSkeleton;
        bool showDistance;
        bool showArrow;
        bool showCallibrationDot;
        double minDistanceFromSensor;
        double maxDistanceFromSensor;
        double testPassRate;

        public AppWindow()
        {
            FormBorderStyle = FormBorderStyle.None;
            WindowState = FormWindowState.Maximized;

            try
            {
                parameters = System.IO.File.ReadAllLines("parameters.txt")
                    .Select(l => l.Split(new[] { '=' }))
                    .ToDictionary(s => s[0].Trim(), s => s[1].Trim());
            }
            catch (Exception)
            {
                Close();
                throw;
            }

            legExpectedAngle = Convert.ToInt32(parameters["legExpectedAngle"]);
            legDevAngleThreshold = Convert.ToInt32(parameters["legDevAngleThreshold"]);
            hipCenterPositionY = Convert.ToInt32(parameters["hipCenterPositionY"]);
            hipCenterDevThreshold = Convert.ToInt32(parameters["hipCenterDevThreshold"]);
            arrowPositionY = Convert.ToInt32(parameters["arrowPositionY"]);
            arrowPoitionYLimit = Convert.ToInt32(parameters["arrowPoitionYLimit"]);
            tiltMinAngle = Convert.ToInt32(parameters["tiltMinAngle"]);
            adjustSensorTilt = Convert.ToBoolean(parameters["adjustSensorTilt"]);
            showSkeleton = Convert.ToBoolean(parameters["showSkeleton"]);
            showDistance = Convert.ToBoolean(parameters["showDistance"]);
            showArrow = Convert.ToBoolean(parameters["showArrow"]);
            showCallibrationDot = Convert.ToBoolean(parameters["showCallibrationDot"]);
            minDistanceFromSensor = Convert.ToDouble(parameters["minDistanceFromSensor"]);
            maxDistanceFromSensor = Convert.ToDouble(parameters["maxDistanceFromSensor"]);
            testPassRate = Convert.ToDouble(parameters["testPassRate"]);

            test = new Test(testPassRate);

            alarmPlayer = new SoundPlayer("alarm.wav");
            alarmThread = new Thread(alarmPlayer.PlaySound);
            backgroundImagePath = "bg1.jpg";

            previousTiltAdjTime = 0;
            showResultBackground = false;

            InitializeComponent();

            this.SetBounds(0, 0, Screen.PrimaryScreen.WorkingArea.Width, Screen.PrimaryScreen.WorkingArea.Height);
            pictureBox.SetBounds(0, 0, Screen.PrimaryScreen.WorkingArea.Width, Screen.PrimaryScreen.WorkingArea.Height);
            pictureBox.Refresh();

            sensor = KinectSensor.KinectSensors[0];
            sensor.ColorStream.Enable(ColorImageFormat.RgbResolution640x480Fps30);
            sensor.DepthStream.Enable(DepthImageFormat.Resolution320x240Fps30);
            sensor.SkeletonStream.Enable();

            sensor.AllFramesReady += FramesReady;
            sensor.Start();
        }

        public double CalculateAngle(Point p1, Point p2, Point p3)
        {
            double distanceAB = Math.Sqrt(Math.Pow(p1.X - p2.X, 2) + Math.Pow(p1.Y - p2.Y, 2));
            double distanceAC = Math.Sqrt(Math.Pow(p1.X - p3.X, 2) + Math.Pow(p1.Y - p3.Y, 2));
            double distanceBC = Math.Sqrt(Math.Pow(p2.X - p3.X, 2) + Math.Pow(p2.Y - p3.Y, 2));
            double angle = Math.Acos((Math.Pow(distanceAB, 2) + Math.Pow(distanceAC, 2) - Math.Pow(distanceBC, 2)) / (2 * distanceAB * distanceAC));
            return (angle * (180 / Math.PI));
        }

        void FramesReady(object sender, AllFramesReadyEventArgs e)
        {
            SkeletonFrame SFrame = e.OpenSkeletonFrame();
            if (SFrame == null)
            {
                return;
            }

            bmap = new Bitmap(System.Drawing.Image.FromFile(backgroundImagePath, true));
            pictureBox.Image = bmap;
            Graphics graph2d = Graphics.FromImage(bmap);

            if (showResultBackground)
            {
                Thread.Sleep(5000);
                showResultBackground = false;
            }

            if (showCallibrationDot)
                DrawCallibrationDot(graph2d, bmap);

            Skeleton[] Skeletons = new Skeleton[SFrame.SkeletonArrayLength];
            SFrame.CopySkeletonDataTo(Skeletons);
            Skeleton S = Skeletons.Where(s => s.TrackingState == SkeletonTrackingState.Tracked).FirstOrDefault();
            if (S == null)
            {
                test.ResetTest();
                backgroundImagePath = "bg1.jpg"; 
                SFrame.Dispose();
                return;
            }
            if (!test.isSkeletonTracked)
            {
                backgroundImagePath = "bg2.jpg";
                //PlaySoundMessage("comm1.wav");
                test.isSkeletonTracked = true;
            }

            if (showSkeleton)
            {
                DrawSkeleton(S, graph2d);
                DrawHead(S, graph2d);
            }
            
            if (showDistance)
            {
                DrawDistance(graph2d, GetBodyDistance(S), S);
            }

            if (adjustSensorTilt)
            {
                AdjustTilt(S, graph2d);
            }
    
            double leftLegAngle = CalculateAngle(GetJoint(JointType.KneeLeft, S), GetJoint(JointType.HipLeft, S), GetJoint(JointType.AnkleLeft, S));
            double rightLegAngle = CalculateAngle(GetJoint(JointType.KneeRight, S), GetJoint(JointType.HipRight, S), GetJoint(JointType.AnkleRight, S));
          
            int legAngleMax = legExpectedAngle + legDevAngleThreshold;
            int legAngleMin = legExpectedAngle - legDevAngleThreshold;

            hipCenterPositionY = GetJoint(JointType.HipCenter, S).X;

            if (test.IsTestRunning() && GetBodyDistance(S) < minDistanceFromSensor)
            {
                test.EndTest();
                PlaySoundMessage("testfinished.wav");
                Thread.Sleep(2000);
                if (test.isUserDrunk())
                {
                    backgroundImagePath = "bgfailure.jpg";
                    bmap = new Bitmap(System.Drawing.Image.FromFile(backgroundImagePath, true));
                    pictureBox.Image = bmap;
                    pictureBox.Refresh();
                    PlaySoundMessage("testfailure.wav");
                    showResultBackground = true;
                }
                else
                {
                    backgroundImagePath = "bgsuccess.jpg";
                    bmap = new Bitmap(System.Drawing.Image.FromFile(backgroundImagePath, true));
                    pictureBox.Image = bmap;
                    pictureBox.Refresh();
                    PlaySoundMessage("testsuccess.wav");
                    showResultBackground = true;
                }
            }
            else
            if ((leftLegAngle < legAngleMin || leftLegAngle > legAngleMax) && test.IsTestRunning())
            { 
                test.SetCurrentDevTime();
                playAlarm();
            }
            else
            if ((rightLegAngle < legAngleMin || rightLegAngle > legAngleMax) && test.IsTestRunning())
            {
                test.SetCurrentDevTime();
                playAlarm();
            }
            else
            if ((hipCenterPositionY > pictureBox.Image.Width / 2 + hipCenterDevThreshold ||
                hipCenterPositionY < pictureBox.Image.Width / 2 - hipCenterDevThreshold)
                && test.IsTestRunning())
            {
                test.SetCurrentDevTime();
                playAlarm();
            }
            else
            if (rightLegAngle > legAngleMin && rightLegAngle < legAngleMax && 
                leftLegAngle > legAngleMin && leftLegAngle < legAngleMax &&
                hipCenterPositionY < pictureBox.Image.Width / 2 + hipCenterDevThreshold &&
                hipCenterPositionY > pictureBox.Image.Width / 2 - hipCenterDevThreshold)
            {
                if (!test.IsTestRunning() && GetBodyDistance(S) > maxDistanceFromSensor)
                {
                    backgroundImagePath = "bgline.jpg";
                    PlaySoundMessage("comm2.wav");
                    Thread.Sleep(2550); // 2550ms - czas na przesłuchanie komunikatu i rozpoczącie testu

                    test.testPathStartDistance = GetBodyDistance(S);
                    test.RunTest();
                }
                test.SumDevTime();
            };

            if (test.IsTestRunning())
            {
                if (showArrow)
                    DrawArrow(graph2d, new Point(pictureBox.Image.Width / 2, arrowPositionY));
                DrawDistance(graph2d, GetBodyDistance(S), S);
                DrawTime(graph2d, test.GetTestTime(), S);
            }

            SFrame.Dispose();
        }

        Point GetJoint(JointType j, Skeleton S)
        {
            SkeletonPoint Sloc = S.Joints[j].Position;
            ColorImagePoint Cloc = sensor.MapSkeletonPointToColor(Sloc, ColorImageFormat.RgbResolution640x480Fps30);

            return new Point(Cloc.X, Cloc.Y);
        }


        Bitmap ImageToBitmap(ColorImageFrame Image)
        {
            byte[] pixelData =
                new byte[Image.PixelDataLength];
            Image.CopyPixelDataTo(pixelData);
            Bitmap bmap = 
                new Bitmap(Image.Width, Image.Height, PixelFormat.Format32bppRgb);
            BitmapData bmapS = bmap.LockBits(
                new Rectangle(0, 0, Image.Width, Image.Height), ImageLockMode.WriteOnly, bmap.PixelFormat);
            IntPtr ptr = bmapS.Scan0;
            Marshal.Copy(pixelData, 0, ptr, Image.PixelDataLength);
            bmap.UnlockBits(bmapS);
            return bmap;
        }

        void playAlarm()
        {
            if (!alarmPlayer.IsPlaying())
                if (!alarmPlayer.IsPlaying())
                {
                    alarmThread = new Thread(alarmPlayer.PlaySound);
                    alarmThread.Start();
                    while (!alarmThread.IsAlive) ;
                }
        }

        void PlaySoundMessage(String filename)
        {
            SoundPlayer player = new SoundPlayer(filename);
            Thread soundMessageThread = new Thread(player.PlaySound);
            soundMessageThread.Start();
            while (!soundMessageThread.IsAlive);
        }

        private void AdjustTilt(Skeleton S, Graphics g)
        {
            long currentTime = DateTime.Now.Ticks / TimeSpan.TicksPerMillisecond;
            if ((currentTime - previousTiltAdjTime) > 1500)
            {
                Point hipPoint = GetJoint(JointType.HipCenter, S);
                int targetHeadHeight = pictureBox.Height / 3;
                int threshold = 30;
                int tiltDevRange = (int) Math.Abs(((targetHeadHeight + threshold) - hipPoint.Y)/10);
                try
                {
                    if (hipPoint.Y > targetHeadHeight + threshold && sensor.ElevationAngle - tiltDevRange > sensor.MinElevationAngle && sensor.ElevationAngle > tiltMinAngle)
                    {
                        sensor.ElevationAngle -= tiltDevRange;
                    }
                    if (hipPoint.Y < targetHeadHeight - threshold && sensor.ElevationAngle + tiltDevRange < sensor.MaxElevationAngle)
                    {
                        sensor.ElevationAngle += tiltDevRange;
                    }
                    previousTiltAdjTime = currentTime;
                }
                catch (InvalidOperationException ex) { }
            }
            //DrawLetter2(g, sensor.ElevationAngle, S);
        }

        public float GetBodyDistance(Skeleton S)
        {
            Joint joint = S.Joints[JointType.HipCenter];
            return joint.Position.Z;   
        }

        private void AppWindow_FormClosing(object sender, FormClosingEventArgs e)
        {
            sensor.Stop();
        }

        private void pictureBox1_Click(object sender, EventArgs e)
        {
            Close();
        }

        void DrawSkeleton(Skeleton S, Graphics g)
        {
            //body
            DrawBone(JointType.Head, JointType.ShoulderCenter, S, g);
            DrawBone(JointType.ShoulderCenter, JointType.Spine, S, g);
            DrawBone(JointType.Spine, JointType.HipCenter, S, g);
            //left leg
            DrawBone(JointType.HipCenter, JointType.HipLeft, S, g);
            DrawBone(JointType.HipLeft, JointType.KneeLeft, S, g);
            DrawBone(JointType.KneeLeft, JointType.AnkleLeft, S, g);
            DrawBone(JointType.AnkleLeft, JointType.FootLeft, S, g);
            //Right Leg
            DrawBone(JointType.HipCenter, JointType.HipRight, S, g);
            DrawBone(JointType.HipRight, JointType.KneeRight, S, g);
            DrawBone(JointType.KneeRight, JointType.AnkleRight, S, g);
            DrawBone(JointType.AnkleRight, JointType.FootRight, S, g);
            //Left Arm
            DrawBone(JointType.ShoulderCenter, JointType.ShoulderLeft, S, g);
            DrawBone(JointType.ShoulderLeft, JointType.ElbowLeft, S, g);
            DrawBone(JointType.ElbowLeft, JointType.WristLeft, S, g);
            DrawBone(JointType.WristLeft, JointType.HandLeft, S, g);
            //Right Arm
            DrawBone(JointType.ShoulderCenter, JointType.ShoulderRight, S, g);
            DrawBone(JointType.ShoulderRight, JointType.ElbowRight, S, g);
            DrawBone(JointType.ElbowRight, JointType.WristRight, S, g);
            DrawBone(JointType.WristRight, JointType.HandRight, S, g);
        }

        void DrawBone(JointType j1, JointType j2, Skeleton S, Graphics g)
        {
            Pen pen = new Pen(Brushes.Magenta, 16.0f);
            Point p1 = GetJoint(j1, S);
            Point p2 = GetJoint(j2, S);
            g.DrawLine(pen, p1, p2);
        }

        void DrawHead(Skeleton S, Graphics g)
        {
            Pen pen = new Pen(Brushes.Blue, 30.0f);
            Point p1 = GetJoint(JointType.Head, S);
            g.DrawEllipse(pen, p1.X - 15, p1.Y - 15, 30, 30);
        }


        void DrawCallibrationDot(Graphics g, Bitmap bmap)
        {
            Pen pen = new Pen(Brushes.Yellow, 10.0f);
            int x = bmap.Width / 2;
            int y = bmap.Height;
            g.DrawEllipse(pen, x - 5, y - 5, 10, 10);
        }

        public void DrawDistance(Graphics g, float distance, Skeleton S)
        {
            Point point = new Point();
            point.X = 20;
            point.Y = 20;
            Brush brush = new SolidBrush(Color.Goldenrod);
            Font font = new Font("Arial", 60.0f);
            g.DrawString(distance.ToString("0.00") + "m", font, brush, point);
        }

        public void DrawTime(Graphics g, long milisecs, Skeleton S)
        {
            milisecs /= 1000;
            Point point = new Point();
            point.X = 650;
            point.Y = 20;
            Brush brush = new SolidBrush(Color.LightSteelBlue);
            Font font = new Font("Arial", 60.0f);
            g.DrawString(milisecs + "s", font, brush, point);
        }

        void DrawArrow(Graphics g, Point p1)
        {
            if (arrowPositionY < arrowPoitionYLimit)
                arrowPositionY = 800;
                arrowPositionY -= 10;
            Pen pen = new Pen(Brushes.Yellow, 15.0f);
            Point p2 = new Point(p1.X, p1.Y + 200);
            Point side1 = new Point(p1.X - 30, p1.Y + 30);
            Point side2 = new Point(p1.X + 30, p1.Y + 30);
            g.DrawLine(pen, p1, p2);
            g.DrawLine(pen, side1, p1);
            g.DrawLine(pen, side2, p1);
        }


    }
}
